#include "myInit.h"


extern uint8_t gAim_Sign;

/* 运球任务 ***************************************************************/
uint8_t gNetRasingFlag = 0;
void dribbleTask(void *argument)
{
	static enum DribbleMode sDribbleCMD = eDribble_Defense;
	static MotorToCAN sMotorToCAN;
	static ShootCMD sShootCMD;
	static uint8_t sDribble_Defense  = 0;
	
	Defense.pf->speedMode(&Defense, -60, 40);
	osDelay(1000);
	Defense.pf->currentDutyMode(&Defense, 10);
	Defense.pf->setOrigin(&Defense);
	
	for(;;)
	{
		switch(sDribbleCMD)
		{	
			case eDribble_One:
				gNetRasingFlag = 1;
				NetR.pf->speedMode(&NetR, 500, 50);
				osDelay(40);
				NetL.pf->speedMode(&NetL, -500, 50);
				osDelay(860);
				NetR.pf->speedMode(&NetR, 200, 50);
				osDelay(1);
				NetL.pf->speedMode(&NetL, -200, 50);
				osDelay(300);
			
				while (1)
				{
					if (ABS(NetL.status.speed) <= 10.f)
					{
						NetL.pf->currentDutyMode(&NetL, -10);
						osDelay(1);
					} else 
					{
						NetL.pf->speedMode(&NetL, -100, 70);
						osDelay(1);
					}
					
					if (ABS(NetR.status.speed) <= 10.f)
					{
						NetR.pf->currentDutyMode(&NetR, 10);
						osDelay(1);
					} else 
					{
						NetR.pf->speedMode(&NetR, 100, 70);
						osDelay(1);
					}
					
					if (ABS(NetR.status.speed) <= 20.f && ABS(NetL.status.speed) <= 20.f)
					{
						NetL.pf->currentDutyMode(&NetL, -10);
						osDelay(1);
						NetR.pf->currentDutyMode(&NetR, 10);
						break;
					}
				}

				break;
				
			case eDribble_Two:
				gNetRasingFlag = 0;
				NetL.pf->speedMode(&NetL, 500, 40);
				osDelay(1);
				NetR.pf->speedMode(&NetR, -500, 40);
				osDelay(900);
				NetL.pf->speedMode(&NetL, 300, 40);
				osDelay(1);
				NetR.pf->speedMode(&NetR, -300, 40);
				osDelay(200);
			
				while (1)
				{
					NetL.pf->speedMode(&NetL, 120, 40);
					osDelay(1);
					NetR.pf->speedMode(&NetR, -120, 40);
					osDelay(1);
					if (ABS(NetR.status.speed) <= 20.f || ABS(NetL.status.speed) <= 20.f)
					{
						break;
					}
				}
				
				NetL.pf->currentDutyMode(&NetL, 10);
				osDelay(1);
				NetR.pf->currentDutyMode(&NetR, -10);
				
				break;

			case eDribble_Defense:
				if (sDribble_Defense == 0)
				{
					Defense.pf->posMode(&Defense, 170, 60, 40);
					sDribble_Defense = 1;
				} else 
				{
					Defense.pf->posMode(&Defense, 30, 60, 40);
					sDribble_Defense = 0;
				}				
				
				break;

			default:	break;	
		}
		osDelay(1);
		xQueueReceive(Dribble_CMDHandle,&sDribbleCMD,portMAX_DELAY);
	}
}

/* 推射任务 ***************************************************************/

void shootTask(void *argument)
{
	static uint8_t encFasle20 = 0;
	static ShootCMD sShootCMD;
	sShootCMD.Mode = eShoot_Catch;
	sShootCMD.Duty = 0;
	static uint8_t sMotorBreakBool = 1;
	
	for(;;)
	{						//解刹车
	#if SHOOT_ENABLE
		osDelay(100);
		while (xQueueReceive(Shoot_CMDHandle,&sShootCMD,1) != pdTRUE)
		{
			while (FDCAN1->PSR & FDCAN_PSR_BO) 
            {
                HAL_FDCAN_Stop(&hfdcan1);
                HAL_FDCAN_DeInit(&hfdcan1);
                HAL_FDCAN_Stop(&hfdcan2);
                HAL_FDCAN_DeInit(&hfdcan2);
                HAL_FDCAN_Stop(&hfdcan3);
                HAL_FDCAN_DeInit(&hfdcan3);
                __HAL_RCC_FDCAN_CLK_DISABLE();
                __HAL_RCC_FDCAN_FORCE_RESET();
                __HAL_RCC_FDCAN_RELEASE_RESET();
                __HAL_RCC_FDCAN_CLK_ENABLE();
                osDelay(10);
                HAL_FDCAN_Init(&hfdcan1);
                HAL_FDCAN_ConfigGlobalFilter(&hfdcan1, FDCAN_ACCEPT_IN_RX_FIFO0, FDCAN_ACCEPT_IN_RX_FIFO0, FDCAN_FILTER_REMOTE, FDCAN_FILTER_REMOTE);
                HAL_FDCAN_ActivateNotification(&hfdcan1, FDCAN_IT_RX_FIFO0_NEW_MESSAGE, 0);
                HAL_FDCAN_Start(&hfdcan1);
                HAL_FDCAN_Init(&hfdcan2);
                HAL_FDCAN_ConfigGlobalFilter(&hfdcan2, FDCAN_ACCEPT_IN_RX_FIFO0, FDCAN_ACCEPT_IN_RX_FIFO0, FDCAN_FILTER_REMOTE, FDCAN_FILTER_REMOTE);
                HAL_FDCAN_ActivateNotification(&hfdcan2, FDCAN_IT_RX_FIFO0_NEW_MESSAGE, 0);
                HAL_FDCAN_Start(&hfdcan2);
                HAL_FDCAN_Init(&hfdcan3);
                HAL_FDCAN_ConfigGlobalFilter(&hfdcan3, FDCAN_ACCEPT_IN_RX_FIFO0, FDCAN_ACCEPT_IN_RX_FIFO0, FDCAN_FILTER_REMOTE, FDCAN_FILTER_REMOTE);
                HAL_FDCAN_ActivateNotification(&hfdcan3, FDCAN_IT_RX_FIFO0_NEW_MESSAGE, 0);
                HAL_FDCAN_Start(&hfdcan3);
                osDelay(10);
            }
			
			osDelay(1);
			if (sMotorBreakBool)
			{
				Shoot.pf->staticBreakMode(&Shoot, 10);
			}
		}
		
		sMotorBreakBool = 0;
		
		switch(sShootCMD.Mode)
		{		
			case eShoot_Bottom:
				Shoot.pf->dutyMode(&Shoot, 0);
				clutchUnBreak();
				osDelay(400);
				while(1)
				{
					if (gShootNowAngle > 10.f)
					{
						Shoot.pf->speedMode(&Shoot, -100.f);
					}
					else
					{
						Shoot.pf->dutyMode(&Shoot, 0);
						clutchBreak();
						osDelay(100);
						clutchUnBreak();
						break;
					}
					osDelay(1);
				}
				break;
				
			case eShoot_Catch:
				while(1)
				{
					if ((gShootNowAngle - cCatchAngle) > cDeathZone)
					{
						Shoot.pf->dutyMode(&Shoot, -10.f);
					}
					else if ((gShootNowAngle - cCatchAngle) < (-cDeathZone))
					{
						Shoot.pf->dutyMode(&Shoot, 10.f);
					}
					else
					{
						Shoot.pf->staticBreakMode(&Shoot, 10);
						clutchBreak();
						osDelay(100);
						clutchUnBreak();
						sMotorBreakBool = 1;
						break;
					}
					osDelay(1);
				}
				break;
				
			case eShoot_Push:
				
				if (gNetRasingFlag == 0)
				{
					break;
				}
			
				if (gShootNowAngle > 12.f)
				{
					break;
				}
				
				if (sShootCMD.Duty > 80.f)
				{
					sShootCMD.Duty = 80;
				}
				
				while(1)
				{       
					static float sLastAngle = 0;
								
					if (sLastAngle == gShootNowAngle)
					{
						encFasle20++;
						if (encFasle20 >= 20)
						{
							Shoot.pf->dutyMode(&Shoot, 0);
							clutchBreak();      			//刹车
							osDelay(500);								//刹车时间, 暂不改
							clutchUnBreak();							//解刹车
							osDelay(500);								//取消刹车时间, 确保刹车取消, 暂不改
							//break之前之前总延时必须超过1000ms确保电机解刹车
							gAim_Sign = 0;
							break;
						}
					} else 
					{
						encFasle20 = 0;
					}
					
					if (gShootNowAngle < cShootAngle)
					{
						Shoot.pf->dutyMode(&Shoot, DEBUGMODE);
					} 
/* 提前量 ***************************************************************/					
//					else if ((gShootNowAngle >= cShootAngle) && (gShootNowAngle < cShootAngle + 400))
//					{
//						clutchBreak();      						//刹车
//					}
/* 提前量 ***************************************************************/					
					else
					{
						Shoot.pf->dutyMode(&Shoot, 0);
						clutchBreak();      						//刹车
						osDelay(500);								//刹车时间, 暂不改
						clutchUnBreak();							//解刹车
						osDelay(500);								//取消刹车时间, 确保刹车取消, 暂不改
						//break之前之前总延时必须超过1000ms确保电机解刹车
						gAim_Sign = 0;
						
						xQueueReceive(Shoot_CMDHandle,&sShootCMD,1);		//清空队列里东西
						sShootCMD.Mode = eShoot_Catch; // 发出指令让推射电机到底部  有bug
						xQueueSend(Shoot_CMDHandle, &sShootCMD, portMAX_DELAY);
						break;
					}
					sLastAngle = gShootNowAngle;
					osDelay(1);
				}
				break;
				
			default:	break;	
		}
	
	#endif	
		xSemaphoreGive(ShootFinishHandle);
		osDelay(1);
	}
}
